package es.uji.viselab.sceneview;

import java.awt.Dimension;
import java.awt.event.MouseEvent;
import java.awt.event.MouseListener;
import java.awt.event.MouseMotionListener;
import java.awt.event.MouseWheelEvent;
import java.awt.event.MouseWheelListener;
import java.nio.DoubleBuffer;
import java.util.ArrayList;
import java.util.Iterator;

import javax.media.opengl.GL;
import javax.media.opengl.GLAutoDrawable;
import javax.media.opengl.GLCanvas;
import javax.media.opengl.GLEventListener;
import javax.media.opengl.glu.GLU;
import javax.swing.JFrame;
import javax.swing.JPanel;
import javax.vecmath.Vector3d;
import com.sun.opengl.util.Animator;
import com.sun.opengl.util.GLUT;

import es.uji.viselab.experiment.DataArrivalListener;
import es.uji.viselab.experiment.Experiment;
import es.uji.viselab.experiment.ExperimentParameters;
import es.uji.viselab.math.Matrix4;
import es.uji.viselab.math.Vector2;
import es.uji.viselab.math.Vector3;
import es.uji.viselab.model.ModelCreator;
import es.uji.viselab.robot.Robot;
import es.uji.viselab.world.World;

public class SimpleRobotScene extends SceneView implements GLEventListener, MouseWheelListener,
		MouseListener, MouseMotionListener {

	GLCanvas canvas;
	JPanel panel;
	GLUT glut;
	GL gl;

	private float viewRotx = 0f, viewRoty = 0f, viewRotz = 0.0f;
	private float viewPosR = 3.464f, viewPosTheta = 0.9553f, viewPosFi = 0.7854f;
	// private float cameraPosx = 0.0f, cameraPosy = 0.0f,cameraPosz = 0.0f;
	// private float cameraYaw=0.0f,cameraPitch=0.0f,cameraRoll=0.0f;
	private float viewDepth = 0f;
	private int prevMouseX, prevMouseY;
	private int prevMouseDespX, prevMouseDespY;
	boolean mouseRButtonDown = false;
	private ArrayList<Vector3> endEffectorCoordinatesList;

	//Matrix4 robotPose = new Matrix4();
	//Matrix4 cameraPose = new Matrix4();
	//Vector3[] modelFeature;

	public SimpleRobotScene(ExperimentParameters ep){
		super(ep);
		endEffectorCoordinatesList = new ArrayList<Vector3>();
	}
	/*public SimpleRobotScene(Experiment experiment) {
		cameraPose = experiment.getCameraParameters().getPose();
		modelFeature = experiment.getModel3d();
		endEffectorCoordinatesList = new ArrayList<Vector3>();
	}*/

	@Override
	public JPanel getPanel() {
		panel = new JPanel();
		createCanvas();
		panel.add(canvas);
		return panel;
	}

	public void createCanvas() {

		canvas = new GLCanvas();
		canvas.addGLEventListener(this);
		canvas.setSize(500, 500);
		final Animator animator = new Animator(canvas);
		animator.start();
	}

	@Override
	public void init(GLAutoDrawable drawable) {
		gl = drawable.getGL();
		glut = new GLUT();
		gl.glClearColor(1f, 1f, 1f, 0.0f);
		gl.glShadeModel(GL.GL_FLAT);

		drawable.addMouseListener(this);
		drawable.addMouseMotionListener(this);
		drawable.addMouseWheelListener(this);
	}

	@Override
	public void mouseDragged(MouseEvent e) {
		int x = e.getX();
		int y = e.getY();
		Dimension size = e.getComponent().getSize();

		if (e.isShiftDown()) {
			float thetaY = ((float) (x - prevMouseDespX)) / 100;
			float thetaX = ((float) (y - prevMouseDespY)) / 100;
			// System.out.println("thetaX " + thetaX + " thetaY " + thetaY);

			prevMouseDespX = x;
			prevMouseDespY = y;

			viewPosTheta += thetaX;
			viewPosFi += thetaY;
		} else {

			float thetaY = 360.0f * ((float) (x - prevMouseX) / (float) size.width);
			float thetaX = 360.0f * ((float) (prevMouseY - y) / (float) size.height);

			prevMouseX = x;
			prevMouseY = y;

			viewRotx += thetaX;
			viewRoty -= thetaY;

		}
	}


	@Override
	public void display(GLAutoDrawable drawable) {

		 //System.out.println("Display!!!");
		gl = drawable.getGL();
		GLU glu = new GLU();
		GLUT glut = new GLUT();
		//
		gl.glClear(GL.GL_COLOR_BUFFER_BIT);

		gl.glColor3f(0.5f, 0.5f, 0.5f);
		gl.glLoadIdentity(); /* clear the matrix */

		// Rotate the entire assembly of gears based on how the user
		// dragged the mouse around
		gl.glRotatef(viewRotx, 1.0f, 0.0f, 0.0f);
		gl.glRotatef(viewRoty, 0.0f, 1.0f, 0.0f);
		gl.glRotatef(viewRotz, 0.0f, 0.0f, 1.0f);

		/* viewing transformation */
		double viewFromX = viewPosR * Math.sin(viewPosTheta) * Math.cos(viewPosFi);
		double viewFromY = viewPosR * Math.sin(viewPosTheta) * Math.sin(viewPosFi);
		double viewFromZ = viewPosR * Math.cos(viewPosTheta);

		glu.gluLookAt(viewFromX, viewFromY, viewFromZ, 0, 0, 0, 0, 0, 1);
		gl.glScalef(1.0f, 1.0f, 1.0f); /* modeling transformation */

		drawAxis();

		Matrix4 cameraPose = ep.getCamParameters().getPose();
		Matrix4 targetPose = ep.getTarget();

		if (robotPose != null && cameraPose!=null) {
			drawEndEffector(robotPose);
			Matrix4 pose = new Matrix4();
			pose.mul(robotPose, cameraPose);
			drawCamera(pose);
			drawModelFeature();
		}
		if (targetPose!=null){
			drawTargetPose(targetPose);
		}
		drawTrajectory();

		gl.glFlush();

	}

	private void drawTargetPose(Matrix4 targetPose) {
		gl.glPushMatrix();

		DoubleBuffer db = targetPose.getDoubleBuffer();
		gl.glMultMatrixd(db);
		gl.glBegin(GL.GL_LINES);
		gl.glColor3f(1, 0, 0); // red
		gl.glVertex3f(0, 0, 0);
		gl.glVertex3f(0.5f, 0, 0); // x
		gl.glColor3f(0, 1, 0); // green
		gl.glVertex3f(0, 0, 0);
		gl.glVertex3f(0, 0.5f, 0); // y
		gl.glColor3f(0, 0, 1); // blue
		gl.glVertex3f(0, 0, 0);
		gl.glVertex3f(0, 0, 1f); // z
		gl.glEnd();

		gl.glPopMatrix();
		
	}
	private void drawModelFeature() {
        Vector3[] modelFeature = ModelCreator.get3d(ep.getModel(),ep.getCamParameters(),ep.getTarget());
		if (modelFeature != null) {
			for (int i = 0; i < modelFeature.length; i++) {
				gl.glColor3f(1, 0, 0); 
				gl.glTranslated(modelFeature[i].x, modelFeature[i].y, modelFeature[i].z);
				glut.glutSolidSphere(0.01, 5, 5);
				gl.glTranslated(-modelFeature[i].x, -modelFeature[i].y, -modelFeature[i].z);
			}
		}

	}

	private void drawEndEffector(Matrix4 pose) {

		gl.glPushMatrix();

		DoubleBuffer db = pose.getDoubleBuffer();
		gl.glMultMatrixd(db);
		gl.glTranslatef(0, 0, -0.2f);

		glut.glutSolidCylinder(0.1f, 0.2f, 5, 5);

		gl.glTranslatef(0, 0, 0.2f);

		gl.glBegin(GL.GL_LINES);
		gl.glColor3f(1, 0, 0); // red
		gl.glVertex3f(0, 0, 0);
		gl.glVertex3f(0.5f, 0, 0); // x
		gl.glColor3f(0, 1, 0); // green
		gl.glVertex3f(0, 0, 0);
		gl.glVertex3f(0, 0.5f, 0); // y
		gl.glColor3f(0, 0, 1); // blue
		gl.glVertex3f(0, 0, 0);
		gl.glVertex3f(0, 0, 1f); // z
		gl.glEnd();

		gl.glPopMatrix();

	}

	private void drawTrajectory() {
		Iterator<Vector3> iterator = endEffectorCoordinatesList.iterator();
		Vector3 previous = null;
		Vector3 pos = null;
		while (iterator.hasNext()) {
			pos = iterator.next();
			if (previous != null) {
				gl.glBegin(GL.GL_LINES);
				gl.glColor3d(0, 0, 0); // black
				gl.glVertex3d(pos.x, pos.y, pos.z);
				gl.glVertex3d(previous.x, previous.y, previous.z); // x
				gl.glEnd();
			}
			previous = new Vector3(pos.x, pos.y, pos.z);
		}
	}

	private void drawCamera(Matrix4 pose) {

		gl.glPushMatrix();

		DoubleBuffer db = pose.getDoubleBuffer();
		// gl.glMatrixMode(GL.GL_MODELVIEW);
		gl.glMultMatrixd(db);

		// The cone has to be drawn in the correct Z direction
		gl.glRotated(-360 / 2, 0, 1.0f, 0);
		gl.glTranslatef(0, 0, -0.2f);
		glut.glutSolidCone(0.1f, 0.2f, 5, 5);
		gl.glTranslatef(0, 0, 0.2f);
		gl.glRotated(+360 / 2, 0, 1.0f, 0);

		gl.glBegin(GL.GL_LINES);
		gl.glColor3f(1, 0, 0); // red
		gl.glVertex3f(0, 0, 0);
		gl.glVertex3f(0.5f, 0, 0); // x
		gl.glColor3f(0, 1, 0); // green
		gl.glVertex3f(0, 0, 0);
		gl.glVertex3f(0, 0.5f, 0); // y
		gl.glColor3f(0, 0, 1); // blue
		gl.glVertex3f(0, 0, 0);
		gl.glVertex3f(0, 0, 1f); // z
		gl.glEnd();

		gl.glPopMatrix();

	}

	private void drawAxis() {
		gl.glBegin(GL.GL_LINES);

		gl.glColor3f(0.5f, 0.5f, 0.5f); // grey
		for (float i = 0; i <= 2; i = i + 0.1f) {
			gl.glVertex3f(i, -1f, 0);
			gl.glVertex3f(i, +1f, 0); // x
		}
		for (float i = -1f; i <= 1; i = i + 0.1f) {
			gl.glVertex3f(0, i, 0);
			gl.glVertex3f(2f, i, 0); // x
		}
		gl.glColor3f(1, 0, 0); // red
		gl.glVertex3f(0, 0, 0);
		gl.glVertex3f(10, 0, 0); // x

		gl.glColor3f(0.5f, 0.5f, 0.5f); // grey
		for (float i = 0; i <= 2; i = i + 0.1f) {
			gl.glVertex3f(0, -0.1f, i);
			gl.glVertex3f(0, +0.1f, i); // x
		}
		for (float i = 0; i <= 2; i = i + 0.1f) {
			gl.glVertex3f(-0.1f, 0, i);
			gl.glVertex3f(+0.1f, 0, i); // x
		}

		gl.glColor3f(0, 1, 0); // green
		gl.glVertex3f(0, 0, 0);
		gl.glVertex3f(0, 10, 0); // y
		gl.glColor3f(0, 0, 1); // blue
		gl.glVertex3f(0, 0, 0);
		gl.glVertex3f(0, 0, 10); // z

		gl.glEnd();
		gl.glFlush();
	}

	@Override
	public void displayChanged(GLAutoDrawable arg0, boolean arg1, boolean arg2) {
		// TODO Auto-generated method stub

	}

	@Override
	public void reshape(GLAutoDrawable drawable, int x, int y, int w, int h) {
		GL gl = drawable.getGL();
		//
		gl.glMatrixMode(GL.GL_PROJECTION); /* prepare for and then */
		gl.glLoadIdentity(); /* define the projection */
		gl.glFrustum(-1.0, 1.0, -1.0, 1.0, 1, 20.0); /* transformation */
		gl.glMatrixMode(GL.GL_MODELVIEW); /* back to modelview matrix */
		gl.glViewport(0, 0, w, h); /* define the viewport */

	}

	@Override
	public void mouseWheelMoved(MouseWheelEvent e) {

		// viewDepth = viewDepth + e.getUnitsToScroll();
		float units = e.getUnitsToScroll();
		viewPosR = viewPosR + units / 10;
		System.out.println("units: " + units + " viewPosR: " + viewPosR);
	}

	
	@Override
	public void robotConnected(Robot robot) {
		robotPose = robot.getPoseM();
		if (robotPose != null) {
			Vector3 v = new Vector3(robotPose.m03, robotPose.m13, robotPose.m23);
			endEffectorCoordinatesList.add(v);
		}
	}

	@Override
	public void robotPositionChanged(Robot robot) {
		robotPose = robot.getPoseM();
		Vector3 v = new Vector3(robotPose.m03, robotPose.m13, robotPose.m23);
		endEffectorCoordinatesList.add(v);

	}

	

	@Override
	public void mouseMoved(MouseEvent e) {
		// TODO Auto-generated method stub
	}

	@Override
	public void mouseClicked(MouseEvent e) {
		// TODO Auto-generated method stub

	}

	@Override
	public void mouseEntered(MouseEvent e) {
		// TODO Auto-generated method stub

	}

	@Override
	public void mouseExited(MouseEvent e) {
		// TODO Auto-generated method stub

	}

	@Override
	public void mousePressed(MouseEvent e) {
		// System.out.println("MousePressed " + e.getX() + " " + e.getY());
		prevMouseX = e.getX();
		prevMouseY = e.getY();
		prevMouseDespX = e.getX();
		prevMouseDespY = e.getY();
		if ((e.getModifiers() & e.BUTTON3_MASK) != 0) {
			mouseRButtonDown = true;
		}
	}

	@Override
	public void mouseReleased(MouseEvent e) {
		// System.out.println("MouseReleased " + e.getX() + " " + e.getY());
		if ((e.getModifiers() & e.BUTTON3_MASK) != 0) {
			mouseRButtonDown = false;
		}
	}

	@Override
	public void worldChanged(World world) {
		ep.setTarget(world.getTargetPose());
	}

	@Override
	public String getPanelTitle() {
		return "Simple scene";
	}

	@Override
	public int panelWidth() {
		return 500;
	}

	@Override
	public int panelHeight() {
		return 500;
	}

}
